#ifndef BFSZX_H
#define BFSZX_H

#pragma once

#include <vector>
#include <utility>
#include <queue>
#include <iostream>
#include <atomic>
#include <stdexcept>
#include <algorithm>

#include <mapbase.h>

void huisu (Coord<int> point, const std::atomic<bool>& interrupt, std::vector<Coord<int>>& path, Coord<int>& start, std::vector<std::vector<std::pair<Coord<int>, Coord<int>>>>& parent, std::vector<std::vector<Coord<int>>>& result);

inline std::vector<std::vector<Coord<int>>> bfszx(
    std::vector<std::vector<int>> graph,
    Coord<int> start,
    Coord<int> end,
    const std::atomic<bool>& interrupt)
{
    std::vector<std::vector<Coord<int>>> result;

    if (graph.empty() || graph[0].empty()) {
        return result;
    }

    int width = graph.size();
    int height = graph[0].size();

    // 检查起点和终点是否有效
    if (start.x < 0 || start.x >= width || start.y < 0 || start.y >= height ||
        end.x < 0 || end.x >= width || end.y < 0 || end.y >= height) {
        return result;
    }

    // 记录访问状态和父节点信息，用于重建路径
    std::queue<Coord<int>> edges;
    std::vector<std::vector<std::pair<Coord<int>, Coord<int>>>> parent(width, std::vector<std::pair<Coord<int>,Coord<int>>>(height, {{0,0}, {0,0}}));

    edges.push(start);
    parent[start.x][start.y] = {{0,0},{0,0}};

    auto extend = [&](Coord<int> edge, int x, int y){
        if(edge.x>=0 && edge.x<=width-1 && edge.y>=0 && edge.y<=height-1
            && edge.x+x>=0 && edge.x+x<=width-1 && edge.y+y>=0 && edge.y+y<=height-1
            && graph[edge.x+x][edge.y+y] != Blocks::wall)
        {
            if((parent[edge.x][edge.y].first+Coord<int>{x,y}) == Coord<int>::zero
                || (parent[edge.x][edge.y].second+Coord<int>{x,y}) == Coord<int>::zero)
                return;
            if(parent[edge.x+x][edge.y+y].first == Coord<int>::zero)
            {
                parent[edge.x+x][edge.y+y].first = {x,y};
                edges.push({edge.x+x, edge.y+y});
            }else if(parent[edge.x+x][edge.y+y].second == Coord<int>::zero)
                parent[edge.x+x][edge.y+y].second = {x,y};
            // else throw std::runtime_error("extend with 3");
            else return;
        }
    };

    while (!edges.empty()) {
        if (interrupt.load()) {
            throw std::runtime_error("Algorithm interrupted by user");
        }

        // 探索边界
        extend(edges.front(), -1, 0); // left
        extend(edges.front(), 1, 0); // right
        extend(edges.front(), 0, -1); // up
        extend(edges.front(), 0, 1); // down
        edges.pop();
    }

    std::vector<Coord<int>> path;

    // auto huisu = [&](auto&& self, Coord<int> point){
    //     if (interrupt.load()) {
    //         throw std::runtime_error("Algorithm interrupted by user");
    //     }
    //     path.push_back(point);
    //     if(point == start)
    //         result.push_back(path);
    //     else
    //     {
    //         if(point==start) return;
    //         if(parent[point.x][point.y].first != Coord<int>::zero)
    //             self(self,point - parent[point.x][point.y].first);
    //         if(parent[point.x][point.y].second != Coord<int>::zero)
    //             self(self,point - parent[point.x][point.y].second);
    //     }
    //     path.pop_back();
    // };

    // huisu(huisu,end);

    huisu(end, interrupt, path, start, parent, result);

    return result;
}

inline void huisu (Coord<int> point, const std::atomic<bool>& interrupt, std::vector<Coord<int>>& path, Coord<int>& start, std::vector<std::vector<std::pair<Coord<int>, Coord<int>>>>& parent, std::vector<std::vector<Coord<int>>>& result){
        if (interrupt.load()) {
            throw std::runtime_error("Algorithm interrupted by user");
        }
        path.push_back(point);
        if(point == start)
            result.push_back(path);
        else
        {
            if(point==start) return;
            if(parent[point.x][point.y].first != Coord<int>::zero)
                huisu(point - parent[point.x][point.y].first,interrupt,path,start,parent,result);
            if(parent[point.x][point.y].second != Coord<int>::zero)
                huisu(point - parent[point.x][point.y].second,interrupt,path,start,parent,result);
        }
        path.pop_back();
    };

#endif // BFSZX_H